#include <CmdMessenger.h>
#include <OneWire.h>
#include <DallasTemperature.h>

// Data wire is plugged into port 2 on the Arduino
#define ONE_WIRE_BUS 3

// Setup a oneWire instance to communicate with any OneWire devices (not just Maxim/Dallas temperature ICs)
OneWire oneWire(ONE_WIRE_BUS);

// Pass our oneWire reference to Dallas Temperature. 
DallasTemperature sensors(&oneWire);

//DeviceAddress insideThermometer;

// Attach a new CmdMessenger object to the default Serial port
CmdMessenger cmdMessenger = CmdMessenger(Serial);

bool pinModeArrays[50] = {false};

enum
{
  // Commands
  WriteGpioValue         , // Command to acknowledge that cmd was received
  WriteGpioValueRet               , // Command to report errors

  ReadGpioValue,
  ReadGpioValueRet,
  
  Temperature       , // Command to request add two floats
  TemperatureRet , // Command to report addition result
  kError,
};

bool pinModeSet(int pin, int mode) {
  if (pinModeArrays[pin])
    return;

  pinMode(pin, mode);
  pinModeArrays[pin] = true;
}

void attachCommandCallbacks()
{
  // Attach callback methods
  cmdMessenger.attach(OnUnknownCommand);
  cmdMessenger.attach(WriteGpioValue, OnWriteGpioValue);
  cmdMessenger.attach(ReadGpioValue, OnReadGpioValue);
  cmdMessenger.attach(Temperature, OnTemperature);
}

void OnUnknownCommand()
{
  cmdMessenger.sendCmd(kError,"Command without attached callback");
}

void OnWriteGpioValue()
{
  int pin = cmdMessenger.readBinArg<int>();
  int value = cmdMessenger.readBinArg<int>();

  pinModeSet(pin, OUTPUT);
  
  digitalWrite(pin, value);
  
  cmdMessenger.sendBinCmd(WriteGpioValueRet, value);
}

void OnReadGpioValue()
{
  int pin = cmdMessenger.readBinArg<int>();

  pinModeSet(pin, INPUT);
  
  int value = digitalRead(pin);
  
  cmdMessenger.sendBinCmd(ReadGpioValueRet, value);
}

void OnTemperature()
{
  sensors.requestTemperatures();
  float tempC = sensors.getTempCByIndex(0);
  cmdMessenger.sendBinCmd(TemperatureRet, tempC);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  attachCommandCallbacks();
  
  // Start up the library
  sensors.begin();
//  sensors.getAddress(insideThermometer, 0);
}

void loop() {
  // put your main code here, to run repeatedly:
  cmdMessenger.feedinSerialData();
}
